#include "control.h"
#define BODY_LEN 1.0   //车身长
#define Radar_e 0 //0 未安装雷达 1 安装雷达（当检测到雷达异常时退出）
/**
 * @brief  返回目标角度与当前角度的锐角差值，带符号， +:顺时针转动, -:逆时针转动
* @note
* @param  cur_lat:
* @param  cur_lon:
* @param  des_lat:
* @param  des_lon:
* @param  des_lat_pre:
* @param  des_lon_pre:
* @retval
*/
 
double get_direction_err(double des_lat, double des_lon, double cur_lat, double cur_lon)
{
	double cur_dir;
	double err;

	get_heading_angle(&cur_dir);
	cur_dir = fmod(cur_dir, 360);


	double tar_dir = get_target_direction(des_lat, des_lon, cur_lat, cur_lon); //获取目标方向
    printf("tar_dir:%f\tcur_dir:%f\n", tar_dir, cur_dir);
	err = tar_dir - cur_dir;

	// 顺时针为正方向
	// err 		 : -180 <= err <= 180
	// err - 360 : err > 180
	// err + 360 : err < -180
	err = fabs(err) <= 180 ? err : err - (err / fabs(err)) * 360;

	return err;
}

double get_path_err(double des_lat_pre, double des_lon_pre, double des_lat, double des_lon, double cur_lat, double cur_lon)
{
	double err;

	double des_dir = get_target_direction(des_lat_pre, des_lon_pre, des_lat, des_lon); //获取上一个点和下一个点的方向
	double tar_dir = get_target_direction(des_lat, des_lon, cur_lat, cur_lon); //获取小车点和目标点的方向

	printf("des_dir:%f\ttar_dir:%f\n", des_dir, tar_dir);
	err = des_dir - tar_dir;

	// 顺时针为正方向
	// err 		 : -180 <= err <= 180
	// err - 360 : err > 180
	// err + 360 : err < -180
	err = fabs(err) <= 180 ? err : err - (err / fabs(err)) * 360;

	return err;
}


double VELOCITY=260;  //车子匀速走速度

double kd = 200;		//距离P
double ka = 20;		//角度P
double ko = 80;		//障碍物P



double P = 0;//路线纠正P
double D = 0;//路线纠正D

double des_lat_pre = 0;
double des_lon_pre = 0;
double des_lat_min = 0;
double des_lon_min = 0;
double path_pre = 0;
double dis_min = 0;
bool circle_flag = 1;//原地转弯标志位
bool ob_con=0;//障碍物确认标志位

bool entry_flag = 0;//入线标志位
bool gps_flag = true;

int tank_control(double cur_lat, double cur_lon, double des_lat, double des_lon)
{
	double l_vel = 0;
	double r_vel = 0;
	double dis_err = 0;
	double ang_err = 0;
	double obs_err = 0;
    double path = 0;
	double path_err = 0;

	dis_err = get_distance_err(des_lat, des_lon, cur_lat, cur_lon);		//获取距离差
	ang_err = get_direction_err(des_lat, des_lon, cur_lat, cur_lon);	//获取角度差
	if(Radar_e){
	obs_err = get_radar_info();											//获取障碍物的信息
	}
	

	//建立储存上一个点的中间变量
	if (gps_flag)//初始化仅运行一次
	{
		des_lat_min = des_lat;
		des_lat_pre = des_lat_min;
		des_lon_min = des_lon;
		des_lon_pre = des_lon_min;
		gps_flag = false;
	}
	if (des_lat_min != des_lat && des_lat_pre == des_lat)//第一个点追到之后更新中间变量，前变量不变还是第一个点
	{
		des_lat_min = des_lat;
		des_lon_min = des_lon;
	}
	if (des_lat_min != des_lat)//第二个点追到之后更新前变量等于第二个点，中间变量以及目标点都变为第三个点
	{
		des_lat_pre = des_lat_min;
		des_lon_pre = des_lon_min;

		des_lat_min = des_lat;
		des_lon_min = des_lon;
	}
if(Radar_e){
if(obs_err < -1)
	{
		set_speed(0, 0);
		printf("radar err\n");

		return 0;
	}
}
	

	if(fabs(dis_err) < 0.15)			//到达目标点
	{
		set_speed(0, 0);
		circle_flag = 1;//打开原地转弯
		entry_flag = 0;//关闭入线标志
		return 1;
	}

	if (des_lat_pre == des_lat)//第一个点按照以前的方案去逐点 path=0
		path = 0;
	else
	{
		path = get_path_err(des_lat_pre, des_lon_pre, des_lat, des_lon, cur_lat, cur_lon);
		path = (path >= 0 ? 1 : -1) * ((log(1 + fabs(path))) - 5.195) * 100;
	}
	
	path_err = path_pre - path;
	ang_err = (ang_err >=0 ? 1 : -1) * log(1 + fabs(ang_err));
	
      
	
	if(Radar_e){
	/*障碍物确定标志位，以防误判*/
    if(obs_err!=0&&ob_con==0)
    { 
        ob_con=1;
        obs_err=0;
    }
    if (obs_err != 0&&ob_con==1)//有障碍物
	{		
		circle_flag=0;//关闭原地转完
		dis_min = dis_err;//记住障碍物所在位置//有新的障碍物会更新障碍物位置
	}
	}
	



	if(circle_flag==0&&(dis_min-dis_err)>=BODY_LEN)//当检测障碍物后距离减少大概车身长度时打开原地转弯
    {
		circle_flag=1;//打开原地转弯
		entry_flag = 1;//打开入线标志
    }

	if (entry_flag == 1)//执行入线程序:执行之前不改变角度调节，执行时角度差肯定在20以内，此时令角度差为0，执行入线，当车身转过一定角度后停止角度调节和入线，此时车直走入线
	{  
		ka = 0;//角度调节等于0
		P=-10;
                VELOCITY=150;
		if (path < 3)
		{
			entry_flag = 0;//当入线基本完成关闭入线程序
			ka = 20;//角度调节恢复
                        ob_con=0;
                        VELOCITY=260;
		}
		else 
		{
			if (fabs(ang_err) > 15)//当车身转为接近垂直，停止入线，直线行驶入线  15只是一个随便写的值，具体还需要测试
			{
				P=0;
			}
		}
		

	}
	
	
	if (fabs(ang_err) >= 2.5 && circle_flag == 1&&entry_flag==0)//原地转弯
	{
		dis_err = 0;
		ka = 30;
		path = 0;
	}
	
	
	//顺时针转动为正  obs_err == 0 无障碍
	if (dis_err < 2.0)
	{
		l_vel = pow(1 - fabs(obs_err), 2) * (kd * log(1 + dis_err) + ka * ang_err + (P * path + D * path_err)) + ko * obs_err;
		r_vel = pow(1 - fabs(obs_err), 2) * (kd * log(1 + dis_err) - ka * ang_err - (P * path + D * path_err)) - ko * obs_err;
	}
	else
	{
		l_vel = pow(1 - fabs(obs_err), 2) * (VELOCITY  + ka * ang_err + (P * path + D * path_err)) + ko * obs_err;
		r_vel = pow(1 - fabs(obs_err), 2) * (VELOCITY  - ka * ang_err - (P * path + D * path_err)) - ko * obs_err;
		
	}
	

	printf("dis_min:%f\tdis_err:%f\nang_err: %f\tpath：%f\nentry_flag:%d\n", dis_min, dis_err, ang_err, path, entry_flag);
	printf("circle_flag: %d\n", circle_flag);

        r_vel=-r_vel;
        l_vel=-l_vel;
	set_speed((int)l_vel, (int)r_vel);

	return 0;
}
